#! /usr/bin/env python

PACKAGE = 'obstacle_avoidance_planner'

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator ()

# trajectory total/fixing length
gen.add ("trajectory_length", double_t, 0, "total trajectory length[m]", 200, 0, 400)
gen.add ("forward_fixing_distance", double_t, 0, "forward fixing length from base_link[m]", 5.0, 0, 100.0)
gen.add ("backward_fixing_distance", double_t, 0, "backward fixing length from base_link[m]", 5.0, 0, 10.0)

# clearance for unique points
gen.add ("clearance_for_straight_line_", double_t, 0, "minimum optimizing range around straight points", 0.05, 0.0, 0.5)
gen.add ("clearance_for_joint_", double_t, 0, "minimum optimizing range around joint points", 3.2, 0.0, 10.0)
gen.add ("clearance_for_only_smoothing", double_t, 0, "minimum optimizing range when applygin only smoothing", 0.2, 0.0, 1.0)
gen.add ("clearance_from_object_for_straight", double_t, 0, "minimum clearance from object when judging straight line", 10.0, 0.0, 20.0)

# clearance(distance) when generating trajectory
gen.add ("clearance_from_road", double_t, 0, "clearance from road boundary[m]", 0.2, 0.0, 1.0)
gen.add ("clearance_from_object", double_t, 0, "clearance from object[m]", 1.0, 0.0, 5.0)
gen.add ("min_object_clearance_for_joint", double_t, 0, "minimum clearance from object[m] when judging is_including_only_smooth_range", 3.2, 0.0, 10.0)
gen.add ("extra_desired_clearance_from_road", double_t, 0, "extra desired clearance from road boundary[m]", 0.0, 0.0, 2.0)

# avoiding param
gen.add ("max_avoiding_objects_velocity_ms", double_t, 0, "maximum velocity for avoiding objects[m/s]", 0.5, 0.0, 5.0)
gen.add ("max_avoiding_ego_velocity_ms", double_t, 0, "maximum ego velocity when avoiding objects[m/s]", 6.0, 0.0, 30.0)
gen.add ("center_line_width", double_t, 0, "center line width around path points used for judging that object is required to avoid or not", 1.7, 0.0, 3.0)
gen.add ("acceleration_for_non_deceleration_range", double_t, 0, "assumed acceleration for calculating non deceleration range", 1.0, 0.0, 10.0)

# mpt param
gen.add ("base_point_weight", double_t, 0, "slack weight for lateral error around base_link", 2000, 0, 1000000)
gen.add ("top_point_weight", double_t, 0, "slack weight for lateral error around vehicle-top point", 1000, 0, 1000000)
gen.add ("mid_point_weight", double_t, 0, "slack weight for lateral error around the middle point between p0 and p1", 1000, 0, 1000000)
gen.add ("lat_error_weight", double_t, 0, "weight for lateral error", 10, 0, 1000000)
gen.add ("yaw_error_weight", double_t, 0, "weight for yaw error", 0, 0, 1000000)
gen.add ("steer_input_weight", double_t, 0, "weight for steering input", 0, 0, 1000000)
gen.add ("steer_rate_weight", double_t, 0, "weight for steering rate", 100, 0, 1000000)
gen.add ("steer_acc_weight", double_t, 0, "weight for steering acceration", 0.000001, 0, 1000000)

exit (gen.generate (PACKAGE, "obstacle_avoidance_planner", "AvoidancePlanner"))
